using System.Collections.Generic;
using UnityEngine;

namespace Core
{
    public class AStarSearch
    {
        public List<Node> OptmPath = new(); // ����·����ֻ�����յ㣩
        public Dictionary<Node, Node> CameFrom = new(); // ����·�����ں�����·���㣩
        public Dictionary<Node, float> CostSoFar = new(); // ������ʷ

        private SquareGrid graph;
        private AStarHeap frontier = new((lhs, rhs) => lhs.Priority <= rhs.Priority);

        private float Heuristic(Node a, Node b)
        {
            return Mathf.Abs(a.X - b.X) + Mathf.Abs(a.Y - b.Y);
        }

        public AStarSearch(SquareGrid g)
        {
            graph = g;
        }

        /// <summary>
        /// ·��������������
        /// </summary>
        /// <param name="start">��ʼλ��</param>
        /// <param name="goal">Ŀ��λ��</param>
        public void Search(Node start, Node goal, IAStarCost cost)
        {
            CameFrom.Clear();
            CostSoFar.Clear();
            
            frontier.Clear();
            frontier.Push(start, 0);

            CameFrom[start] = start;
            CostSoFar[start] = 0;

            while (frontier.Count > 0)
            {
                var current = frontier.Pop();

                if (current.Equals(goal))
                {
                    ReconstructPath(start, goal);
                    break;
                }

                var iter = graph.Neighbors(current).GetEnumerator();
                while (iter.MoveNext())
                {
                    var next = iter.Current;
                    // float newCost = CostSoFar[current]
                    //                 + graph.Cost(current, next);
                    float newCost = CostSoFar[current] + cost.Cost(current, next);
                    if (!CostSoFar.ContainsKey(next)
                        || newCost < CostSoFar[next])
                    {
                        CostSoFar[next] = newCost;
                        float priority = newCost + Heuristic(next, goal);
                        frontier.Push(next, priority);
                        CameFrom[next] = current;
                    }
                }
            }
        }

        /// <summary>
        /// ����·���㣬ֻ�����յ�
        /// </summary>
        private void ReconstructPath(Node start, Node goal)
        {
            OptmPath.Clear();
            var cur = goal;
            int curDeltaX = 0;
            int curDeltaZ = 0;
            while (CameFrom.ContainsKey(cur) && cur != start)
            {
                var parent = CameFrom[cur];
                var deltaX = cur.X - parent.X;
                var deltaZ = cur.Y - parent.Y;
                if (deltaX != curDeltaX || deltaZ != curDeltaZ)
                {
                    OptmPath.Add(cur);
                    curDeltaX = deltaX;
                    curDeltaZ = deltaZ;    
                }
                cur = parent;
            }
            OptmPath.Add(start);
        }
    }
}